#include <libplayerc++/playerc++.h>
#include <joystick.h>
#include <kalman.h>

#define RED     0xff0000
#define GREEN   0x00ff00
#define BLUE    0x0000ff
#define CYAN    0x00ffff
#define MAGENTA 0xff00ff
#define YELLOW  0xffff00
#define GRAY    0x7f7f7f

#define MAX_RANGE   8.0

#define LEFT    ((360-(*bp)[blob_index].left))
#define RIGHT   ((360-(*bp)[blob_index].right))
#define START   (((RIGHT < LEFT) ? RIGHT : LEFT) > 0) ? ((RIGHT < LEFT) ? RIGHT : LEFT) : 0;
#define END     (((LEFT > RIGHT) ? LEFT : RIGHT) > 0) ? ((LEFT > RIGHT) ? LEFT : RIGHT) : 0;

#define REF_ANGLE   (double) (360-bp[i].x)/2

int countBlobs(PlayerCc::BlobfinderProxy *bp);
int getLandmarkID (int color);
double getLandmarkRange(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index);
double getLandmarkBearing(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index);
const char* getLandmarkSignature(PlayerCc::BlobfinderProxy *bp, int blob_index);
